from wormbane.geometry import Box, Vector, manhattan_distance
from heapq import heappush, heappop, heapify
from math import sqrt


class Pathfinder(object):

    def __init__(self, size):

        # Cells are created lazily.
        self.grid = [[None for y in xrange(size.y)] for x in xrange(size.x)]

        self.time = 0
        self.open_list = []

    def __call__(self, start_pos, goal_pos, neighbors, cost, heuristic):

        # Increment time.
        time = self.time + 1
        self.time = time

        # The open list is a priority queue of open cells.
        open_list = self.open_list
        del open_list[:]

        # Add the start cell to the open list.
        grid = self.grid
        start_cell = grid[start_pos.x][start_pos.y]
        if start_cell is None:
            # Lazy cell creation.
            start_cell = _Cell(start_pos)
            grid[start_pos.x][start_pos.y] = start_cell
        id = 1
        start_cell.reinit(time, id, heuristic(start_pos), open=True)
        heappush(open_list, start_cell)

        # Track the cell closest to goal so far.
        best_cell = start_cell

        # Process the open list.
        while open_list:

            # Process the cell having the lowest f = g + h.
            current_cell = heappop(open_list)
            current_cell.open = False

            # Are we there yet?
            if current_cell.pos == goal_pos:
                while current_cell is not start_cell:
                    yield current_cell.pos
                    current_cell = current_cell.parent
                return

            # Process the neighbors of the current cell.
            for neighbor_pos in neighbors(current_cell.pos):
                neighbor_cell = grid[neighbor_pos.x][neighbor_pos.y]
                if neighbor_cell is None:
                    # Lazy cell creation.
                    neighbor_cell = _Cell(neighbor_pos)
                    grid[neighbor_pos.x][neighbor_pos.y] = neighbor_cell
                neighbor_g = (current_cell.g
                              + cost(current_cell.pos, neighbor_pos))
                if neighbor_cell.time != time:
                    id += 1
                    neighbor_cell.reinit(time, id, heuristic(neighbor_pos),
                                         neighbor_g, current_cell, True)
                    heappush(open_list, neighbor_cell)
                    if neighbor_cell.h < best_cell.h:
                        best_cell = neighbor_cell
                elif neighbor_g < neighbor_cell.g:
                    # Found a shorter path to the neighbor. Update the
                    # neighbor, possibly breaking the heap invariant.
                    neighbor_cell.parent = current_cell
                    neighbor_cell.g = neighbor_g
                    if neighbor_cell.open:
                        # Reestablish the heap invariant.
                        heapify(open_list)
                    else:
                        # Reopen the neighbor.
                        neighbor_cell.open = True
                        heappush(open_list, neighbor_cell)

        # Return the best path.
        while best_cell is not start_cell:
            yield best_cell.pos
            best_cell = best_cell.parent


class _Cell(object):
    def __init__(self, pos):
        self.pos = pos
        self.time = 0
        self.id = 0
        self.h = 0
        self.g = 0
        self.parent = None
        self.open = False

    def __cmp__(self, other):
        # Order by f = g + h, using ID to break ties.
        return (cmp(self.g + self.h, other.g + other.h)
                or cmp(self.id, other.id))

    def reinit(self, time=0, id=0, h=0, g=0, parent=None, open=False):
        self.time = time
        self.id = id
        self.h = h
        self.g = g
        self.parent = parent
        self.open = open


def _test():

    size = Vector(79, 20)
    bounds = Box(Vector(0, 0), size)
    find_path = Pathfinder(size)
    sqrt_2 = sqrt(2)

    start_pos = Vector(10, 5)
    goal_pos = Vector(70, 15)

    def neighbors(pos):
        for x in xrange(pos.x - 1, pos.x + 2):
            for y in xrange(pos.y - 1, pos.y + 2):
                if x != pos.x or y != pos.y:
                    try:
                        yield Vector(x, y)
                    except IndexError:
                        pass

    def cost(a, b):
        return 1 if a.x == b.x or a.y == b.y else sqrt_2

    def heuristic(pos):
        return abs(goal_pos.x - pos.x) + abs(goal_pos.y - pos.y)

    def opened(x, y):
        cell = find_path.grid[x][y]
        return cell is not None and cell.time == find_path.time

    path = list(find_path(start_pos, goal_pos, neighbors, cost, heuristic))
    lines = [[('?' if opened(x, y) else ' ')
              for x in xrange(size.x)] for y in xrange(size.y)]
    lines[start_pos.y][start_pos.x] = '@'
    for pos in path:
        lines[pos.y][pos.x] = '*'
    print '\n'.join(''.join(line) for line in reversed(lines))


if __name__ == '__main__':
    _test()
